#include "application.hpp"
#include <iostream>
#include <utils.hpp>
#include <windowFilter.hpp>
class Net_can_current_APP : public Application
{
    float rpmL = 0;
    float rpmR = 0;
    Recorder recorder;
    NetServer server;
    PID anglePID;
    PID speedPID;
    PID rotatePID;
    WindowFilter filter = WindowFilter(10);

    int count = 0;
    int countPrint = 0;
    int countRecord = 0;
    int countServer = 0;

    float deltaMs = 0;
    float avrRPM = 0;
    float lastInput = 0;

    float speedCtrlAngle = 0;

    bool OnTick(Input input) override
    {
        using namespace std;

        float moveInput = server.GetValueOrDefault("move", 0.0f);
        float rotateInput = server.GetValueOrDefault("rotate", 0.0f);
        input.move = moveInput * config.moveAix.range;
        input.rotate = -rotateInput * config.rotateAix.range;
        input.move = input.move * 0.2 + lastInput * 0.8;
        lastInput = input.move;

        // speed control
        deltaMs += state.deltaTimeMs;
        // avrRPM = avrRPM * 0.4 + 0.6 * (rpmL + rpmR) / 2;
        filter.PushNewItem((rpmL + rpmR) / 2);
        avrRPM = filter.Result();
        FrequencyDividerInvoke(10, count, [this, input] {
            float speedError = avrRPM - input.move;
            // speed loop is positive
            speedCtrlAngle = -speedPID.GetControl(speedError, deltaMs);
            speedCtrlAngle = ABSClamp(speedCtrlAngle, config.maxAngle);
            deltaMs = 0;
        });

        // IMU read
        // angle in state forward is negative(-),backward is positive(+)
        IMUState IMUState = device.imu.GetState();
        // now angle forward is positive(+),backward is negative(-)
        float angle = -IMUState.angle[1] + config.zeroPoint;
        float gyro = IMUState.gyro[1];
        float gyroz = IMUState.gyro[2];
        CheckValueRangeAndExit(angle, -config.errorAngle, config.errorAngle, "angle");

        // angle control loop
        // forward is positive(+),backward is negative(-)
        float angleError = angle - speedCtrlAngle;
        // float angleError = angle - input.move;
        float ctrl = anglePID.GetControlWithDError(angleError, gyro, state.deltaTimeMs);
        ctrl = ABSClamp(ctrl, config.maxOutput);
        float rotateCtrl = rotatePID.GetControl(gyroz - input.rotate, state.deltaTimeMs);
        rotateCtrl = ABSClamp(rotateCtrl, config.maxOutput * 0.2f);
        rpmL = SetMotorAndGet(ctrl - rotateCtrl, device.motorCanLeft);
        rpmR = SetMotorAndGet(ctrl + rotateCtrl, device.motorCanRight);

        Application::Message message{{"rpm", avrRPM}, {"cA", speedCtrlAngle}, {"angle", angle},
                                     {"gyro", gyro},  {"ctrl", ctrl},         {"input", input.move}};

        FrequencyDividerInvoke(5, countPrint, [this, message] { PrintOut(message); });
        FrequencyDividerInvoke(1, countRecord, [this, message] { RecordAll(recorder, message); });
        // FrequencyDividerInvoke(1, countRecord, [this, message] {  ServerUpload(server, message); });
        return true;
    }

    virtual bool Init() override
    {
        server.Open(config.socket.ip, config.socket.port, 3);
        server.ChangeFrequent(10);

        anglePID.SetP(config.anglePID.P);
        anglePID.SetI(config.anglePID.I);
        anglePID.SetD(config.anglePID.D);
        anglePID.Reset();

        speedPID.SetP(config.speedPID.P);
        speedPID.SetI(config.speedPID.I);
        speedPID.SetD(config.speedPID.D);
        speedPID.Reset();

        rotatePID.SetP(config.rotatePID.P);
        rotatePID.SetI(config.rotatePID.I);
        rotatePID.SetD(config.rotatePID.D);
        rotatePID.Reset();

        recorder.SetPath("/home/orangepi/dev/self-balance-proto-mi9/data/data.csv");
        bool success = Application::Init();

        SetTickInterval(5);
        std::this_thread::sleep_for(std::chrono::seconds(2));

        return success;
    }

    void Quit() override
    {
        server.Close();
        recorder.Save();
        Application::Quit();
    }

    float SetMotorAndGet(float value, Motor_Can &motor)
    {
        using namespace std;

        value = ABSClamp(value, config.maxOutput);
        motor.RequestRPM();
        motor.SendHeartBeat();
        motor.SetCurrent(value);
        float rpm = motor.GetRPM();
        CheckValueRangeAndExit(rpm, -config.errorRPM, config.errorRPM, "rpm");
        return rpm;
    }
};

int main()
{
    std::string configFilePath = "/home/orangepi/dev/self-balance-proto-mi9/config/protoMi9_CAN_Current_Position.yaml";

    Net_can_current_APP app;
    try
    {
        app.SimpleStart(configFilePath);
    }
    catch (std::string error)
    {
        std::cout << error << std::endl;
        return -1;
    }
    return 0;
}
